#!/usr/bin/env python

import signal
import pyexotica as exo
from numpy import array
from pyexotica.publish_trajectory import *
from time import sleep

import exotica_scipy_solver

exo.Setup.init_ros()
problem = exo.Setup.load_problem(
    '{exotica_examples}/resources/configs/example_ik_constrained.xml')

# solver = exotica_scipy_solver.SciPyEndPoseSolver(problem=problem, method='trust-constr', debug=False)
solver = exotica_scipy_solver.SciPyEndPoseSolver(problem=problem, method='SLSQP', debug=False)

dt = 0.002
t = 0.0
q = array([0.0]*7)
print('Publishing IK')
signal.signal(signal.SIGINT, sig_int_handler)
while True:
    try:
        problem.start_state = q
        problem.start_time = t
        q = solver.solve()[0]
        publish_pose(q, problem, t)
        sleep(dt)
        t = (t+dt) % 7.0
    except KeyboardInterrupt:
        break
